// OpenCV Sample Application: facedetect.c

// Include header files
#include "cv.h"
#include "highgui.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <math.h>
#include <float.h>
#include <limits.h>
#include <time.h>
#include <ctype.h>
#include <iostream>
using namespace std;

#define min(a,b) ((a)>(b))?(a):(b)

// Create memory for calculations
static CvMemStorage* storage = 0;

// Create a new Haar classifier
static CvHaarClassifierCascade* cascade = 0;

// Function prototype for detecting and drawing an object from an image
void detect_and_draw( IplImage* image );

// Create a string that contains the cascade name
const char* cascade_name =
    "haarcascade_frontalface_alt.xml";

// me :D --------------------------------------------
// Compute Pc_s
void addFunction(IplImage* img, CvPoint p1, CvPoint p2);
void showtemp(IplImage *);

IplImage* temp1;
float skin[256][256][256] = {0};
float nonskin[256][256][256] = {0};
bool b = true;
float Ps;
int key = 1;
//--------------------------------------------

// Main function, defines the entry point for the program.
int main()	
{
    // Structure for getting video from camera or avi
    CvCapture* capture = 0;
    // Images to capture the frame from video or camera or from file
    IplImage *frame, *frame_copy = 0;
    // Load the HaarClassifierCascade
    cascade = (CvHaarClassifierCascade*)cvLoad( cascade_name, 0, 0, 0 );
    // Check whether the cascade has loaded successfully. Else report and error and quit
    if( !cascade )
    {
        fprintf( stderr, "ERROR: Could not load classifier cascade\n" );
        return -1;
    }
    
    // Allocate the memory storage
    storage = cvCreateMemStorage(0);    
	
	capture = cvCreateCameraCapture(0);
    // Create a new named window with title: result
    cvNamedWindow( "result", 1 );
	cvNamedWindow( "temp1", 1 );

    // Find if the capture is loaded successfully or not.

    // If loaded succesfully, then:
    if( capture )
    {
        // Capture from the camera.
        for(;;)
        {
            // Capture the frame and load it in IplImage
            if( !cvGrabFrame( capture )) 
				break;
            frame = cvRetrieveFrame( capture );
            // If the frame does not exist, quit the loop
            if( !frame )
                break;          
            // Allocate framecopy as the same size of the frame
            if( !frame_copy )
                frame_copy = cvCreateImage( cvSize(frame->width,frame->height), IPL_DEPTH_8U, frame->nChannels );

            // Check the origin of image. If top left, copy the image frame to frame_copy. 
            if( frame->origin == IPL_ORIGIN_TL )
                cvCopy( frame, frame_copy, 0 );
            // Else flip and copy the image
            else
                cvFlip( frame, frame_copy, 0 );
            
			temp1 = cvCreateImage( cvSize(frame->width,frame->height), 8, 1 );
            
			// Call the function to detect and draw the face
            detect_and_draw( frame_copy );
		
            // Wait for a while before proceeding to the next frame
			key = cvWaitKey(33);
			if (key == 27) break;
			showtemp(frame);
        }

        // Release the images, and capture memory
        cvReleaseImage( &frame_copy );
        cvReleaseCapture( &capture );
    }
    // Destroy the window previously created with filename: "result"
    cvDestroyWindow("result");
	cvDestroyWindow("temp1");

    return 0;
}

// Function to detect and draw any faces that is present in an image
void detect_and_draw( IplImage* img )
{
    int scale = 1;

    // Create a new image based on the input image
    IplImage* temp = cvCreateImage( cvSize(img->width/scale,img->height/scale), 8, 3 );

    // Create two points to represent the face locations
    CvPoint pt1, pt2;
    int i;

    // Clear the memory storage which was used before
    cvClearMemStorage( storage );

    // Find whether the cascade is loaded, to find the faces. If yes, then:
    if( cascade )
    {

        // There can be more than one face in an image. So create a growable sequence of faces.
        // Detect the objects and store them in the sequence
        CvSeq* faces = cvHaarDetectObjects( img, cascade, storage,
                                            1.1, 2, CV_HAAR_DO_CANNY_PRUNING,
                                            cvSize(40, 40) );

        // Loop the number of faces found.
        for( i = 0; i < (faces ? faces->total : 0); i++ )
        {
           // Create a new rectangle for drawing the face
            CvRect* r = (CvRect*)cvGetSeqElem( faces, i );

            // Find the dimensions of the face,and scale it if necessary
            pt1.x = r->x*scale + 15;
            pt2.x = (r->x+r->width)*scale - 15;
            pt1.y = r->y*scale + 40;
            pt2.y = (r->y+r->height)*scale - 15;

            // Draw the rectangle in the input image
            cvRectangle( img, pt1, pt2, CV_RGB(255,0,0), 3, 8, 0 );

			// Them vao :|
			key = cvWaitKey(33);
			if (b && key == 97)
				addFunction(img, pt1, pt2);
        }
    }

    // Show the image in the window named "result"
    cvShowImage( "result", img );

    // Release the temp image created.
    cvReleaseImage( &temp );
}

void addFunction(IplImage* img, CvPoint p1, CvPoint p2)
{
	b = false;
	for( int y=0; y<img->height; y++ ) 
	{
		uchar* ptr = (uchar*) (img->imageData + y * img->widthStep);
		for( int x=0; x<img->width; x++ ) 
		{
			int r = ptr[3*x + 2];
			int g = ptr[3*x + 1];
			int b = ptr[3*x + 0];
			if (p1.y <= y && y <= p2.y && p1.x <= x && x <= p2.x)
				skin[r][g][b] = skin[r][g][b] + 1;
			else
				nonskin[r][g][b] = nonskin[r][g][b] + 1;
		}
	}

	int k = (p2.x - p1.x) * (p2.y - p1.y);
	int nk = img->width * img->height - k;
	Ps = float(k) / (k + nk);
	int mr, mg, mb;
	mr = mg = mb = 0;
	for (int r = 0; r <= 255; r++)
		for (int g = 0; g <= 255; g++)
			for (int b = 0; b <= 255; b++)
			{
				if (r > g && r > b && r > 30 && min(g, b) < 235)
				{
					skin[r][g][b] = skin[r][g][b] / k;
					if (skin[r][g][b] > skin[mr][mg][mb])
					{
						mr = r;
						mg = g;
						mb = b;
					}
				}
				else
					skin[r][g][b] = 0;
				nonskin[r][g][b] = nonskin[r][g][b] / nk;
			}
}

void showtemp(IplImage *frame)
{
	if (b == false)
	{
		for( int y=0; y<frame->height; y++ ) 
		{
			uchar* ptr = (uchar*) (frame->imageData + y * frame->widthStep);
			uchar* ptr1 = (uchar*) (temp1->imageData + y * temp1->widthStep);
			for( int x=0; x<frame->width; x++ ) 
			{		
				int r = ptr[3*x + 2];
				int g = ptr[3*x + 1];
				int b = ptr[3*x + 0];
				if (nonskin[r][g][b] + skin[r][g][b] != 0)
				{
					float Pc = skin[r][g][b] / (nonskin[r][g][b] + skin[r][g][b]);
					float Ps_c = skin[r][g][b] * Ps;
					if (Ps_c > 0.000001)
						ptr1[x] = 255;
					else
						ptr1[x] = 0;
				}
				else
					ptr1[x] = 0;
			}
		}
	}
	cvShowImage( "temp1", temp1 );
}